/*
 * pidmotor.c
 *
 *  Created on: Mar 14, 2011
 *      Author: ahmedhadjeres*/

#include <avr/io.h>
#include <stdint.h>
#include <avr/interrupt.h>
#include <util/delay.h>
#include "../../source/IRSENSOR/IRSENSOR.h"
#include "../../header/AVR035.h"
#include "../../source/MOTOR/MOTOR.h"

static unsigned int k0, k1, k2;
static unsigned int error_2 = 0, error_1 = 0, error_0 = 0;
static unsigned int output = 0;

void init_PID(unsigned int Kp, unsigned int Ki, unsigned int Kd, unsigned int T) {

	k0 = (Kp + Kp * Ki * T / 2.0 + Kp * Kd / T);
	k1 = (-Kp - 2 * Kp * Kd / T + Ki * Kp * T / 2);
	k2 = (Kp * Kd / T);

}

unsigned int PID(unsigned int error) {
	error_2 = error_1;
	error_1 = error_0;
	error_0 = error;
	output += k0 * error_0 + k1 * error_1 + k2 * error_2;
	return output;

}
